#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTSMUX,         sensorI2CCustom)
#pragma config(Sensor, S3,     liftDownSensor, sensorTouch)
#pragma config(Sensor, S4,     lineFollower,   sensorLightActive)
#pragma config(Motor,  mtr_S1_C1_1,     linearSlides,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     grabberMount,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     frontLeft,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     rearLeft,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     frontRight,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     rearRight,     tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C4_1,    wristMain,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "MenusAndIcons.c"

#include "drivers/lego-touch.h"
#include "drivers/lego-light.h"
#include "drivers/hitechnic-sensormux.h"
#include "drivers/hitechnic-compass.h"
#include "drivers/hitechnic-gyro.h"
#include "drivers/hitechnic-irseeker-v2.h"
#include "drivers/lego-ultrasound.h"

const tMUXSensor HTIRS2 = msensor_S2_3;
const tMUXSensor HTCOMPASS = msensor_S2_2;
const tMUXSensor LEGOUS = msensor_S2_4;
const tMUXSensor HTGYRO = msensor_S2_1;

const string sMechanismPrompt = "Select Mechanism";

const short iconElev = iconForward;
const short iconArm = iconBackRight;
const short iconWrist = iconTurnRight2;

typedef enum
{
	mechElev = 0x00,
	mechArm,
	mechWrist,
	numMechs,
} TMech;

task main()
{
	TMech mech = mechElev;
	TMenuAnimationType anim = menuFromRight;

	drawMenu(anim, iconWrist, iconElev, iconArm, "Elevator", sMechanismPrompt);

	while ( true ) {
		switch (nNxtButtonTransition)
		{
		case kRightButton:
			mech++;
			mech %= numMechs;
			anim = menuFromRight;
			break;

		case kLeftButton:
			mech--;
			mech %= numMechs;
			anim = menuFromLeft;
			break;

		case kEnterButton:
		case kExitButton:
			return;
			break;
		}


		switch ( mech )
		{
		case mechElev:
			drawMenu(anim, iconWrist, iconElev, iconArm, "Elevator", sMechanismPrompt);
			break;
		case mechArm:
			drawMenu(anim, iconElev, iconArm, iconWrist, "Arm", sMechanismPrompt);
			break;
		case mechWrist:
			drawMenu(anim, iconArm, iconWrist, iconElev, "Wrist", sMechanismPrompt);
			break;
		}

	}
}
